/*
 * MODBUS Slave Library: A portable MODBUS slave for MODBUS ASCII/RTU/TCP.
 * Copyright (c) 2008 Christian Walter <cwalter@embedded-solutions.at>
 * Copyright (c) 2020 Embedded Experts GmbH <modbus@embedded-experts.at>
 *
 * All rights reserved. Use of this file is subject to license terms.
 */

/* ----------------------- System includes ----------------------------------*/
#include <stdlib.h>

/* ----------------------- Platform includes --------------------------------*/
#include "mbport.h"

/* ----------------------- Modbus includes ----------------------------------*/
#include "mbs.h"
#include "mbsiframe.h"
#include "mbsi.h"
#include "mbsfunctions.h"

/* ----------------------- Defines ------------------------------------------*/
#define MB_PDU_FUNC_READ_HOLDING_SIZE       ( 4 )
#define MB_PDU_FUNC_READ_ADDR_OFF           ( MB_PDU_DATA_OFF )
#define MB_PDU_FUNC_READ_REGCNT_OFF         ( MB_PDU_DATA_OFF + 2 )
#define MB_PDU_FUNC_READ_HOLDING_REGCNT_MAX ( 0x007D )

#define MB_PDU_FUNC_WRITE_ADDR_OFF  ( MB_PDU_DATA_OFF + 0 )
#define MB_PDU_FUNC_WRITE_VALUE_OFF ( MB_PDU_DATA_OFF + 2 )
#define MB_PDU_FUNC_WRITE_SIZE      ( 4 )

#define MB_PDU_FUNC_MASKWRITE_SIZE     ( 6 )
#define MB_PDU_FUNC_MASKWRITE_ADDR_OFF ( MB_PDU_DATA_OFF + 0 )
#define MB_PDU_FUNC_MASKWRITE_AND_OFF  ( MB_PDU_DATA_OFF + 2 )
#define MB_PDU_FUNC_MASKWRITE_OR_OFF   ( MB_PDU_DATA_OFF + 4 )

#define MB_PDU_FUNC_WRITE_MUL_ADDR_OFF    ( MB_PDU_DATA_OFF + 0 )
#define MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF  ( MB_PDU_DATA_OFF + 2 )
#define MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF ( MB_PDU_DATA_OFF + 4 )
#define MB_PDU_FUNC_WRITE_MUL_VALUES_OFF  ( MB_PDU_DATA_OFF + 5 )
#define MB_PDU_FUNC_WRITE_MUL_SIZE_MIN    ( 5 )
#define MB_PDU_FUNC_WRITE_MUL_REGCNT_MAX  ( 0x007B )

#define MB_PDU_FUNC_READWRITE_READ_ADDR_OFF    ( MB_PDU_DATA_OFF + 0 )
#define MB_PDU_FUNC_READWRITE_READ_REGCNT_OFF  ( MB_PDU_DATA_OFF + 2 )
#define MB_PDU_FUNC_READWRITE_WRITE_ADDR_OFF   ( MB_PDU_DATA_OFF + 4 )
#define MB_PDU_FUNC_READWRITE_WRITE_REGCNT_OFF ( MB_PDU_DATA_OFF + 6 )
#define MB_PDU_FUNC_READWRITE_BYTECNT_OFF      ( MB_PDU_DATA_OFF + 8 )
#define MB_PDU_FUNC_READWRITE_WRITE_VALUES_OFF ( MB_PDU_DATA_OFF + 9 )
#define MB_PDU_FUNC_READWRITE_SIZE_MIN         ( 9 )

/* ----------------------- Type definitions ---------------------------------*/

/* ----------------------- Static variables ---------------------------------*/

/* ----------------------- Static functions ---------------------------------*/

/* ----------------------- Start implementation -----------------------------*/
eMBException
eMBSFuncReadHoldingRegister( UBYTE *pubMBPDU, USHORT *pusMBPDULen,
                             const xMBSRegisterCB *pxMBSRegisterCB ) 
{
    USHORT       usRegAddress;
    USHORT       usRegCount;
    UBYTE *      pubFrameCur;
    eMBException eStatus = MB_PDU_EX_ILLEGAL_DATA_ADDRESS;

    /* Length check */
    if( ( MB_PDU_FUNC_READ_HOLDING_SIZE + MB_PDU_SIZE_MIN ) == *pusMBPDULen )
    {
        usRegAddress = ( USHORT )( ( USHORT )pubMBPDU[MB_PDU_FUNC_READ_ADDR_OFF] << 8 );
        usRegAddress |= ( USHORT )( pubMBPDU[MB_PDU_FUNC_READ_ADDR_OFF + 1] );
        usRegCount = ( USHORT )( ( USHORT )pubMBPDU[MB_PDU_FUNC_READ_REGCNT_OFF] << 8 );
        usRegCount |= ( USHORT )( pubMBPDU[MB_PDU_FUNC_READ_REGCNT_OFF + 1] );
        /* Check if the number of registers to read is valid. If not
         * return Modbus illegal data value exception.
         */
        if( ( usRegCount >= 1 ) && ( usRegCount <= MB_PDU_FUNC_READ_HOLDING_REGCNT_MAX ) )
        {
            /* Set the current PDU data pointer to the beginning. */
            pubFrameCur = &pubMBPDU[MB_PDU_FUNC_OFF];
            *pusMBPDULen = MB_PDU_FUNC_OFF;
            /* First byte contains the function code. */
            *pubFrameCur++ = MBS_FUNCCODE_READ_HOLDING_REGISTERS;
            *pusMBPDULen += ( USHORT )1;

            /* Second byte in the response contain the number of bytes. */
            *pubFrameCur++ = ( UBYTE )( usRegCount * 2 );
            *pusMBPDULen += ( USHORT )1;

            /* Get the acutal register values from the callback. */
            if( NULL != pxMBSRegisterCB->peMBSRegHoldingCB )
            {
                *pusMBPDULen += ( USHORT )( usRegCount * 2 );
#if MBS_CALLBACK_ENABLE_CONTEXT == 1
                eStatus = pxMBSRegisterCB->peMBSRegHoldingCB( pxMBSRegisterCB->pvCtx, pubFrameCur, usRegAddress,
                                                              usRegCount, MBS_REGISTER_READ );

#else
                eStatus =
                    pxMBSRegisterCB->peMBSRegHoldingCB( pubFrameCur, usRegAddress, usRegCount, MBS_REGISTER_READ );
#endif
            }
            else
            {
                eStatus = MB_PDU_EX_ILLEGAL_DATA_ADDRESS;
            }
        }
        else
        {
            eStatus = MB_PDU_EX_ILLEGAL_DATA_VALUE;
        }
    }
    return eStatus;
}

eMBException
eMBSFuncWriteSingleRegister( UBYTE *pubMBPDU, USHORT *pusMBPDULen,
                             const xMBSRegisterCB *pxMBSRegisterCB ) 
{
    USHORT       usRegAddress;
    eMBException eStatus = MB_PDU_EX_ILLEGAL_DATA_ADDRESS;

    if( ( MB_PDU_FUNC_WRITE_SIZE + MB_PDU_SIZE_MIN ) == *pusMBPDULen )
    {
        usRegAddress = ( USHORT )( ( USHORT )pubMBPDU[MB_PDU_FUNC_WRITE_ADDR_OFF] << 8 );
        usRegAddress |= ( USHORT )( pubMBPDU[MB_PDU_FUNC_WRITE_ADDR_OFF + 1] );

        /* Get the acutal register values from the callback. */
        if( NULL != pxMBSRegisterCB->peMBSRegHoldingCB )
        {
#if MBS_CALLBACK_ENABLE_CONTEXT == 1
            eStatus = pxMBSRegisterCB->peMBSRegHoldingCB(
                pxMBSRegisterCB->pvCtx, &pubMBPDU[MB_PDU_FUNC_WRITE_VALUE_OFF], usRegAddress, 1, MBS_REGISTER_WRITE );
#else
            eStatus = pxMBSRegisterCB->peMBSRegHoldingCB( &pubMBPDU[MB_PDU_FUNC_WRITE_VALUE_OFF], usRegAddress, 1,
                                                          MBS_REGISTER_WRITE );
#endif
        }
        else
        {
            eStatus = MB_PDU_EX_ILLEGAL_DATA_ADDRESS;
        }
    }
    else
    {
        /* Can't be a valid request because the length is incorrect. */
        eStatus = MB_PDU_EX_ILLEGAL_DATA_VALUE;
    }
    return eStatus;
}

eMBException
eMBSFuncWriteMultipleHoldingRegister( UBYTE *pubMBPDU, USHORT *pusMBPDULen,
                                      const xMBSRegisterCB *pxMBSRegisterCB ) 
{
    USHORT usRegAddress;
    USHORT usRegCount;
    UBYTE  ubRegByteCount;

    eMBException eStatus = MB_PDU_EX_NONE;

    if( *pusMBPDULen >= ( MB_PDU_FUNC_WRITE_MUL_SIZE_MIN + MB_PDU_SIZE_MIN ) )
    {
        usRegAddress = ( USHORT )( ( USHORT )pubMBPDU[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF] << 8 );
        usRegAddress |= ( USHORT )( pubMBPDU[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF + 1] );

        usRegCount = ( USHORT )( ( USHORT )pubMBPDU[MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF] << 8 );
        usRegCount |= ( USHORT )( pubMBPDU[MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF + 1] );

        ubRegByteCount = pubMBPDU[MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF];

        if( ( NULL != pxMBSRegisterCB->peMBSRegHoldingCB ) && ( usRegCount >= 1 ) &&
            ( usRegCount <= MB_PDU_FUNC_WRITE_MUL_REGCNT_MAX ) && ( ubRegByteCount == ( UBYTE )( 2 * usRegCount ) ) )
        {
#if MBS_CALLBACK_ENABLE_CONTEXT == 1
            eStatus =
                pxMBSRegisterCB->peMBSRegHoldingCB( pxMBSRegisterCB->pvCtx, &pubMBPDU[MB_PDU_FUNC_WRITE_MUL_VALUES_OFF],
                                                    usRegAddress, usRegCount, MBS_REGISTER_WRITE );
#else
            eStatus = pxMBSRegisterCB->peMBSRegHoldingCB( &pubMBPDU[MB_PDU_FUNC_WRITE_MUL_VALUES_OFF], usRegAddress,
                                                          usRegCount, MBS_REGISTER_WRITE );
#endif
            if( MB_PDU_EX_NONE == eStatus )
            {
                /* The response contains the function code, the starting
                 * address and the quantity of registers. We reuse the
                 * old values in the buffer because they are still valid.
                 */
                *pusMBPDULen = MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF;
            }
        }
        else
        {
            eStatus = MB_PDU_EX_ILLEGAL_DATA_VALUE;
        }
    }
    else
    {
        /* Can't be a valid request because the length is incorrect. */
        eStatus = MB_PDU_EX_ILLEGAL_DATA_VALUE;
    }
    return eStatus;
}
